import numpy as np


def inv_rt(rt):
    import copy
    rt_inv = copy.deepcopy(rt)
    rt_inv[:3, :3] = np.linalg.inv(rt[:3, :3])
    rt_inv[:3, 3:4] = -np.linalg.inv(rt[:3, :3]) @ rt[:3, 3:4]
    return rt_inv


def rt2mat(rotation, translation, inverse=True):
    output = np.eye(4)
    output[:3, :3] = rotation
    output[:3, 3] = translation

    if inverse:
        output[:3, :3] = np.linalg.inv(rotation)
        output[:3, 3] = np.linalg.inv(rotation) @ (-translation)
        # output = np.linalg.inv(output)
    return output


def to_matrix4x4(m):
    output = np.eye(4)
    output[:3, :3] = m
    return output


def pad_ones(pts):
    assert pts.shape[1] == 3
    return np.concatenate((pts, np.ones(shape=(len(pts), 1))), axis=1)
